#ifndef TIM_MOUNT_HAOFU_H
#define TIM_MOUNT_HAOFU_H
#include <stdio.h>
#include <stdlib.h>
#include <sys/file.h>
#include <sys/types.h>
#include <sys/statfs.h>
#include <sys/stat.h>
#include <unistd.h>
#include <fcntl.h>
#include <pthread.h>
#include <sys/select.h>
#include <sys/time.h>
#include <signal.h>
#include <termios.h>
#include <errno.h>
#include <string.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <sys/wait.h>
#include <arpa/inet.h>
#include <assert.h>
#include <stdint.h>
#include <sys/ioctl.h>
#include <termios.h>
#include "circular_buf.h"


#define PROTOCOL_DATA_BUFFER_SIZE  64

class MountHaofu
{
public:
	MountHaofu();
	~MountHaofu();

	bool init(const char *ip);

	void update();

	bool udpClient(const char *ip, const char *port);

	void handlerRecv();

	void handlerSend();

	typedef void(*haofuAttitudeCallback)(float pitch, float yaw);

	bool registerHaofuAttitudeCallback(haofuAttitudeCallback function);

	bool mountLinkHealthy();

	typedef struct Protocol{
		// check sum
		uint8_t m_checksum;
		// ID of message 
		uint8_t m_msgid;
		// message length
		uint8_t m_len;
		// save message date
		uint8_t m_data[PROTOCOL_DATA_BUFFER_SIZE];
	} ProtocolType;

	typedef enum {
    	ParseStateWaitHead1 = 0, // 0x55
		ParseStateWaitHead2, // 0xaa
		ParseStateWaitHead3, // 0xdc
    	ParseStateWaitFrameLensAndCount,
    	ParseStateWaitFrameId,
    	ParseStateCollectFrameDate,
    	ParseStateWaiteCrc
	} ParseStateType;

	typedef enum {
    	FramingStateIncomplete 		= 0,
    	FramingStateOk		 		= 1,
    	FramingStateError 			= 2
	}FramingStateType;

	typedef struct ProtocolState {
		// Number of received messages
    	uint8_t m_msgReceived;
		// Parsing state machine
    	ParseStateType m_parseState;
		// Index in current packet
    	uint8_t m_packetIdx;
    	uint16_t m_parseError;
	} ProtocolStateType;


	enum ProtocolMessageIdSendToHaofu{
		SendCommonControlCommands = 0x30,
		SendUncommonControlCommands = 0x31,
		SendAhrsInformation = 0xB1,
		SendSystemStatesControl = 0x13
	};

	enum ProtocolMessageIdRecvFormHaofu{
		RecvCommonCtonrolCommands = 0x40,
		RecvUncommonCtronlCommands = 0x41,
		RecvSystemStates = 0x23
	};


	uint16_t packageProtocolToBuffer(uint8_t *buf, uint8_t msgId,uint8_t *msgData, uint16_t msgLen);

	uint8_t handleProtocolData(ProtocolType *recvMsg, uint8_t c);

	uint8_t ProtocolMessageParseChar(ProtocolType *recvMessage, uint8_t c);


	void process_packet(ProtocolType &msg);


	//haofu commands
	void sendStartMessage();

	void sendCameraDown();

	void manualLockingDirection();

	void centerGimbal();

	void sendFollowUpMessage(int16_t headingSpeed, int16_t pitchSpeed,
												int16_t headingTarget, int16_t pitchTarget);

	void sendZoom(bool add);

	void sendSystemCommand();

	//characters displaying
	void openCharactersDisplay();

	void closeCharactersDisplay();

	//Center Cross Line
	void openCenterCrossLine();

	void closeCenterCrossLine();

	//Bomen display
	void openBomenDisplay();

	void closeBomenDisplay();

	//Field of view angle setting
	void setFieldOfViewAngle();

	void setFieldOfViewAngle(float targetAngle);


	//land assistant control
	bool landAssistantControl();

	void setCopterAhrsToHaofu(float pitch, float roll, float yaw);

	void setCopterGpsToHaofu(float pitch, float roll, float yaw, int32_t lat, int32_t lng, int32_t alt, uint16_t hdop, uint16_t vdop);

private:
	ProtocolType m_protocolDataBuffer;
	ProtocolStateType m_protocolState;


	bool m_initialised;
	uint32_t m_lastCurrentAngleMs; 

	int m_udpSockfd;
	struct sockaddr_in m_saddr;
	pthread_t m_RecvRunTid;
	pthread_t m_SendRunTid;

	struct circularBuffer m_sendBuffer;

	//callback function
	haofuAttitudeCallback m_haofuAttitudeCallbackFunction;

	float m_currentHeading;
	float m_currentPitch;
	bool m_startLiveStreamFlag;
};


extern MountHaofu mountHaofuHandler;

#endif //TIM_MOUNT_HAOFU_H

